/**
  ****************************(C) COPYRIGHT 2024 Polarbear****************************
  * @file       shoot_fric.c/h
  * @brief      使用摩擦轮的发射机构控制器。
  * @note       包括初始化，目标量更新、状态量更新、控制量计算与直接控制量的发送
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Apr-1-2024      Penguin         1. done
  *  V1.0.1     Apr-16-2024     Penguin         1. 完成基本框架
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2024 Polarbear****************************
*/

#include "shoot_fric_trigger.h"
#if (SHOOT_TYPE == SHOOT_FRIC_TRIGGER)
#include "usb_task.h"
#include "usb_debug.h"
#include "CAN_communication.h"
#include "AHRS_middleware.h"
#include "user_lib.h"
#include "referee.h"
#include "detect_task.h"
#include "ui.h"

#define rc_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }

bool fireflag = false;
bool no_shoot = true;
bool last_mouse_press_l = false;
bool last_mouse_press_r = false;
uint8_t fire_ready = 0;
uint16_t firetime = 0;
extern float Get_Yaw_curr(void);//获取yaw控制量，来这里一起发
extern float Get_Pitch_curr(void);//获取pitch控制量，来这里一起发
fp32 tri_fdb=0;
fp32 set = 0;
fp32 trigger_channel = 0;
static Shoot_s SHOOT = {
  .mode = LOAD_STOP,
  .state = FRIC_NOT_READY,
  .shoot_speed = 0.0f,
  .dangle = 0.0f,
  .lastroll = 0,
  .roll = 0,
  .trigger_pid_speed = 
    {
      .Kp = 9000.0f,//9000.0f
      .Ki = 0.0f,//0.0
      .Kd = 5.0f,//5.0

      .max_out = 16000, //最大输出
      .max_iout = 0, //最大积分输出
    },
  .trigger_pid_pos = 
    {
      .Kp = 1.0f,//1.0f
      .Ki = 0.0f,//0.0
      .Kd = 0.5f,//0.05f

      .max_out = 100.0f, //最大输出
      .max_iout = 0.0f, //最大积分输出
    },
  .fric_pid[0] = 
    {
      .Kp = 1000.0f,
      .Ki = 0.0f,
      .Kd = 1.0f,

      .max_out = 10000.0f, //最大输出
      .max_iout = 0.0f, //最大积分输出
    },
  .fric_pid[1] = 
    {
      .Kp = 1000.0f,
      .Ki = 0.0f,
      .Kd = 1.0f,

      .max_out = 10000.0f, //最大输出
      .max_iout = 0.0f, //最大积分输出
    },

};

/*-------------------- Init --------------------*/

/**
 * @brief          初始化
 * @param[in]      none
 * @retval         none
 */
void ShootInit(void) 
{ 
    SHOOT.rc = get_remote_control_point(); 
    MotorInit(&SHOOT.fric_motor[0], 1, 2, DJI_M3508,1,1,1);
    MotorInit(&SHOOT.fric_motor[1], 2, 2, DJI_M3508,1,1,1);
    MotorInit(&SHOOT.trigger_motor, 5, 1, DJI_M3508,1,DJI_GM3508_RR,1);
}

/*-------------------- Set mode --------------------*/

/**
 * @brief          设置模式
 * @param[in]      none
 * @retval         none
 */
void ShootSetMode(void)
{
    if (no_shoot&&switch_is_down(SHOOT.rc->rc.s[1])) {
        SHOOT.mode = LOAD_STOP;
        SHOOT.state = FRIC_NOT_READY;
    } else if (fireflag||switch_is_up(SHOOT.rc->rc.s[1])) {
        SHOOT.mode = FIRE;
        SHOOT.state = FRIC_READY;
    } else if (!fireflag||switch_is_mid(SHOOT.rc->rc.s[1])) {
        SHOOT.mode = AIM;
        SHOOT.state = FRIC_NOT_READY;
    } 
}

/*-------------------- Observe --------------------*/

/**
 * @brief          更新状态量
 * @param[in]      none
 * @retval         none
 */
void ShootObserver(void) 
{
  if (&SHOOT == NULL)
  {
      return;
  }

  GetMotorMeasure(&SHOOT.fric_motor[0]);
  GetMotorMeasure(&SHOOT.fric_motor[1]);
  GetMotorMeasure(&SHOOT.trigger_motor);

  if (SHOOT.rc->key.v & KEY_PRESSED_OFFSET_Q)//按R键一键启用发射模式
  {
      fireflag = true;
      no_shoot = false;
  }
  else if(SHOOT.rc->key.v & KEY_PRESSED_OFFSET_E)//按B键进入无力状态
  {
      fireflag = false;
      no_shoot = false;
  }
  else if(SHOOT.rc->key.v & KEY_PRESSED_OFFSET_B)
  {
      no_shoot = true;
  }

  //输出轴位置
  if(((SHOOT.trigger_motor.fdb.ecd - SHOOT.lastecd)>6000))
  {
    SHOOT.roll--;
  }
  else if ((SHOOT.trigger_motor.fdb.ecd - SHOOT.lastecd)<-6000)
  {
    SHOOT.roll++;
  }

  tri_fdb = SHOOT.trigger_motor.fdb.pos+2*M_PI*SHOOT.roll;
  SHOOT.lastecd = SHOOT.trigger_motor.fdb.ecd;
  SHOOT.last_mode = SHOOT.mode;
  firetime++;//计时
  fire_ready = HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_9);
	

}

/*-------------------- Reference --------------------*/

/**
 * @brief          更新目标量
 * @param[in]      none
 * @retval         none
 */
void ShootReference(void) 
{
  if (&SHOOT == NULL)
  {
      return;
  }
  //fp32 trigger_channel=0;
  //deadline, because some remote control need be calibrated,  the value of rocker is not zero in middle place,
  //死区限制，因为遥控器可能存在差异 摇杆在中间，其值不为0
  rc_deadband_limit(SHOOT.rc->rc.ch[1], trigger_channel, 5);
  set = trigger_channel * RC_TO_ONE * M_PI;//trigger_channeld的值是遥控器映射值范围-660~660;
  if(SHOOT.state == FRIC_READY)
  {
    SHOOT.fric_motor[0].set.vel = -SHOOT_SPEED_MAX;//500
    SHOOT.fric_motor[1].set.vel = SHOOT_SPEED_MAX;
  }
  else if(SHOOT.state == FRIC_NOT_READY)
  {
    SHOOT.fric_motor[0].set.vel = 0.0f;
    SHOOT.fric_motor[1].set.vel = 0.0f;
  }
  if (SHOOT.mode==FIRE)
  {
    //键鼠
    if (last_mouse_press_l == 0)
    {
      if ((SHOOT.rc->mouse.press_l == 1)&&firetime > 1000)
      {
        SHOOT.trigger_motor.set.pos = tri_fdb + (M_PI * 53.2f)/3.0f;//触发电机的减速比为53.2;
        firetime = 0;
      }
    } 
    //遥控
    if (trigger_channel * RC_TO_ONE > 0.8f)
    {
      if ((trigger_channel * RC_TO_ONE > 0.8f)&&firetime > 1000)
      {
        SHOOT.trigger_motor.set.pos = tri_fdb + (M_PI * 53.2f)/3.0f;//触发电机的减速比为53.2;
        firetime = 0;
      }
    } 
    else if (trigger_channel * RC_TO_ONE < -0.8f)
    {
      if ((trigger_channel * RC_TO_ONE < -0.8f)&&firetime > 1000)
      {
        SHOOT.trigger_motor.set.pos = tri_fdb - (M_PI * 53.2f)/3.0f;//触发电机的减速比为53.2;
        firetime = 0;
      }
    } 
  }
  else if(SHOOT.mode==AIM)
  {
    SHOOT.trigger_motor.set.vel = 0.0f;
  }
}


/*-------------------- Console --------------------*/

/**
 * @brief          计算控制量
 * @param[in]      none
 * @retval         none
 */
void ShootConsole(void) 
{
  switch (SHOOT.mode) {
    case LOAD_STOP: 
    {
        SHOOT.fric_motor[0].set.curr = 0.0f;
        SHOOT.fric_motor[1].set.curr = 0.0f;
        SHOOT.trigger_motor.set.curr = 0.0f;
        break;
    }
    case AIM:
    {
        SHOOT.fric_motor[0].set.curr = PID_calc(&SHOOT.fric_pid[0],SHOOT.fric_motor[0].fdb.vel,SHOOT.fric_motor[0].set.vel);
        SHOOT.fric_motor[1].set.curr = PID_calc(&SHOOT.fric_pid[1],SHOOT.fric_motor[1].fdb.vel,SHOOT.fric_motor[1].set.vel);
        SHOOT.trigger_motor.set.curr = PID_calc(&SHOOT.trigger_pid_speed,SHOOT.trigger_motor.fdb.vel,SHOOT.trigger_motor.set.vel);
        break;
    }
    case FIRE:
    {
      //摩擦轮
        PID_calc(&SHOOT.fric_pid[0],SHOOT.fric_motor[0].fdb.vel,SHOOT.fric_motor[0].set.vel);
        PID_calc(&SHOOT.fric_pid[1],SHOOT.fric_motor[1].fdb.vel,SHOOT.fric_motor[1].set.vel);
        SHOOT.fric_motor[0].set.curr = (int16_t)(SHOOT.fric_pid[0].out);
        SHOOT.fric_motor[1].set.curr = (int16_t)(SHOOT.fric_pid[1].out);
      //拨弹
      SHOOT.trigger_motor.set.vel = PID_calc(&SHOOT.trigger_pid_pos,tri_fdb,SHOOT.trigger_motor.set.pos);
      SHOOT.trigger_motor.set.curr = PID_calc(&SHOOT.trigger_pid_speed,SHOOT.trigger_motor.fdb.vel,SHOOT.trigger_motor.set.vel);
        break;
    }
    default:
      break;
  }

  if (toe_is_error(DBUS_TOE))
  {
    SHOOT.fric_motor[0].set.curr = 0.0f;
    SHOOT.fric_motor[1].set.curr = 0.0f;
    SHOOT.trigger_motor.set.curr = 0.0f;
    return;
  }

}

/*-------------------- Cmd --------------------*/

/**
 * @brief          发送控制量
 * @param[in]      none
 * @retval         none
 */
void ShootSendCmd(void) 
{
  CanCmdDjiMotor
    (
        2,0x200,  //两个摩擦轮，can2的1和2
        SHOOT.fric_motor[0].set.curr, 
        SHOOT.fric_motor[1].set.curr,
        Get_Pitch_curr(),//Get_Pitch_curr(), //pitch，can2的3,先暂时别开
        0
    );
  CanCmdDjiMotor
    (
        1,0x1ff,//拨弹3508，can1的5
        SHOOT.trigger_motor.set.curr,
        Get_Yaw_curr(),//yaw，can1的2
        0,
        0
    );
    // ModifyDebugDataPackage(1,SHOOT.trigger_motor.fdb.pos,"fdbp");
    // ModifyDebugDataPackage(2,SHOOT.trigger_motor.set.pos,"rc");
    // ModifyDebugDataPackage(3,SHOOT.trigger_motor.fdb.vel,"fdbv");
    // ModifyDebugDataPackage(4,SHOOT.trigger_motor.set.curr,"curr");
    // ModifyDebugDataPackage(5,SHOOT.trigger_motor.set.vel,"setv");
    // ModifyDebugDataPackage(6,SHOOT.dangle,"chushi");
    // ModifyDebugDataPackage(7,tri_fdb,"fdbp1");
    ModifyDebugDataPackage(1,tri_fdb,"fdb");
    ModifyDebugDataPackage(2,SHOOT.trigger_motor.set.pos,"set");
    ModifyDebugDataPackage(3,SHOOT.trigger_motor.set.pos-tri_fdb,"err");
}    

#endif  // SHOOT_TYPE == SHOOT_FRIC
